from algo import *
import numpy as np

# -------------------------------ORCA START---------------------------
# 随机初始化动态障碍空间
# ORCA算法写完了之后并到一个类中 测试部分封成一个子函数

# 计算u矢量
# 极坐标圆弧和射线
def get_u_orca(v_opt=np.zeros(3), circle=np.zeros(3)):
	'''
	求ORCA的u
	:param v_opt: x,y,z
	:param circle: x,y,r
	:return: u_point,flag
	'''
	# 射线与圆心矢量的夹角
	vc = np.array([circle[0], circle[1], 0])
	c_r = circle[2]
	lvc = norm(vc)
	# print('lvc', lvc)
	# print('alpha', alpha * 180 / pi)
	u_point = np.zeros(3, dtype=float)
	u_vec = np.zeros(3, dtype=float)
	lr = np.zeros(3)
	ll = np.zeros(3)
	flag = False
	if lvc < c_r:
		print('lvc小于半径c_r 已经与目标重叠')
		return u_vec, u_point, flag, ll, lr

	alpha = asin(c_r / lvc)  # 0-pi

	# print('angle_opt_c', angle_opt_c)
	# v_opt在射线的锐角范围内
	# 圆心矢量右侧r
	# 求右侧射线与圆的交点
	lr_norm = vc * cos(alpha)  # 得到圆心矢量方向 模等于切点矢量模的矢量
	print('alpha', alpha)
	# print('lr_norm', lr_norm)
	lr = trans(point=np.array([lr_norm[0], lr_norm[1], 0, 1]),
	           angle=np.array([0, 0, -alpha]),
	           mode=False)
	# 求左侧射线与圆的交点
	ll = trans(point=np.array([lr_norm[0], lr_norm[1], 0, 1]),
	           angle=np.array([0, 0, alpha]),
	           mode=False)

	# 判断v_opt是否在射线之间 以圆心矢量为0度方向
	angle_opt_c = vector_dot_angle(vc, v_opt)
	if abs(angle_opt_c) <= alpha:

		# 以lr为基准轴 如果(v_opt-vc)到基准的角度小于 (ll-vc)的角度 则在扇形内
		# print('ll, lr', ll, lr)
		print('v_opt在锥形范围内')
		ll_vc = ll[0:3] - vc
		lr_vc = lr[0:3] - vc
		v_opt_vc = v_opt - vc
		# print(vc)
		# print('ll_vc', ll_vc)
		# print('lr_vc', lr_vc)

		a_opt = vector_dot_angle(v_opt_vc, ll_vc)
		a_l = vector_dot_angle(lr_vc, ll_vc)
		# print('a_opt', a_opt)
		# print('a_l', a_l)
		# 通过叉乘判断v_opt在圆内还是圆外 判断侧 如果在扇形一侧还要满足半径距离约束 排除锥形内扇形外的情况
		if 0 <= a_opt <= a_l:
			print('v_opt在扇形内')
			if norm(v_opt_vc) <= c_r:
				print('v_opt在扇形范围内')
				# print('v_opt_vc norm', norm(v_opt_vc))
				# 求过圆心和v_opt的射线与圆弧的交点就是VO边界上到v_opt最近的点
				u_point = vc + c_r * v_opt_vc / norm(v_opt_vc)
				flag = True

			else:
				print('不在圆内')
				# 在扇形外 锥形内 靠近原点一侧非VO区域
				flag = False
		else:
			print('v_opt在扇形外侧 锥形内 远离原点一侧的VO区域')
			# 求v_opt到射线的垂线
			# 先求圆心矢量与y轴的夹角 y轴[0，1]
			print('在射线域内')
			angle_c = vector_dot_angle(vc, np.array([0, 1, 0]))
			if 0 < angle_opt_c <= alpha:
				# 靠近lr射线 通过极坐标和矢量内积为0得到
				lr_r = v_opt[0] * sin(angle_c + alpha) + v_opt[1] * cos(angle_c + alpha)
				u_point[0] = lr_r * sin(angle_c + alpha)
				u_point[1] = lr_r * cos(angle_c + alpha)
			else:
				# 靠近ll射线
				ll_r = v_opt[0] * sin(angle_c - alpha) + v_opt[1] * cos(angle_c - alpha)
				u_point[0] = ll_r * sin(angle_c - alpha)
				u_point[1] = ll_r * cos(angle_c - alpha)
			flag = True
	else:
		print('不在锥形内的情况')
		flag = False
	if flag:
		u_vec = u_point - v_opt
	return u_vec, u_point, flag, ll, lr


def get_orca_area(u_vec=np.zeros(3), u_point=np.zeros(3), opt=np.zeros(3), eta=1e-5):
	'''
	计算ORCA半平面代数式
	:param u_point: u点坐标
	:param u_vec: np.array 3*1 ORCA算法的u矢量
	:param v_opt: np.array 3*1
	:return: ORCA半平面直线
	'''
	# 已知u
	u_coe = np.zeros(3, dtype=float)
	l_coe = np.zeros(3, dtype=float)

	# 过u点 与u_vec平行直线方程
	if abs(voc_dot(u_vec, np.array([0, 1, 0]))) < eta:
		# 如果u矢量垂直于vx轴
		print('与纵轴平行 本直线B=0')
		u_coe[0] = 1
		u_coe[1] = 0
		u_coe[2] = u_point[0]
	else:
		print('与纵轴不平行 B不为0')
		u_coe[0] = u_vec[1] / u_vec[0]
		u_coe[1] = -1
		u_coe[2] = u_point[1] - u_coe[0] * u_point[0]
	print('u_coe', u_coe)
	# 求过v_opt + 0.5u的一般方程  这里假设 a b都按ORCA 但实际情况不是 所以不是分摊责任 而是 a负双倍责任
	point = opt + 1 * u_vec
	if abs(u_coe[0]) < eta:
		# 如果过u直线与vx平行 那么本直线与lu垂直
		print(' 与vy平行 B为0')
		l_coe[0] = 1
		l_coe[1] = 0
		l_coe[2] = -point[0]
	else:
		# B不为0
		print('B不为0')
		l_coe[0] = -1 / u_coe[0]
		l_coe[1] = -1
		l_coe[2] = point[1] - l_coe[0] * point[0]
	re = [l_coe, u_vec, point]
	return re


# 求直线一般方程与圆的交点
def line_intersect_circle(a, p):
	# p is the circle parameter, a is the line Ax+By+C=0
	x0, y0, r = p
	A, B, C = a
	eta = 1e-3
	# print('a', a)
	if abs(B)<eta and abs(A)>eta:
		dis = point_to_line(p, a)
		if r >= dis:
			p1 = [-C / A, y0 + sqrt(r ** 2 - (-C / A - x0) ** 2)]
			p2 = [-C / A, y0 - sqrt(r ** 2 - (-C / A - y0) ** 2)]
			inp = [p1, p2]
		else:
			inp = []
	else:
		print(B)
		k = -A / B
		b0 = -C / B
		a = k ** 2 + 1
		b = 2 * k * (b0 - y0) - 2 * x0
		c = (b0 - y0) ** 2 + x0 ** 2 - r ** 2
		delta = b ** 2 - 4 * a * c
		if delta >= 0:
			p1x = (-b - sqrt(delta)) / (2 * a)
			p2x = (-b + sqrt(delta)) / (2 * a)
			p1y = k * p1x + b0
			p2y = k * p2x + b0
			inp = [[p1x, p1y, 0], [p2x, p2y, 0]]
		else:
			inp = []
	return inp

# n和u点处的法向量
def nbody_area(A, v_max, v_now_a, v_perf):
	'''

	:param A: np.array k*6 直线参数Ax+ By+ C=0 和 n矢量共6个float
	:param v_max 圆的半径 中心在原点
	:param v_perf 在v_max内的偏好速度 3维
	:return:
	'''
	# 计算ORCA 求解v_new

	v = v_perf
	B = []
	for h in A:
		flag = in_half_plane(h, v)
		l_h = np.array([h[0], h[1], h[2]])
		n = np.array([h[3], h[4], h[5]])
		if flag:
			# v不变
			print('v不变')
		else:
			h_point = []  # 保存所有

			# 求vperf到l_h的垂足
			l_perf = get_h_line(v_perf, n)
			perf_point, flag_perf = get_cross_point(l_perf, l_h)
			v_flag = point_in_B(perf_point, flag_perf, v_max, B)

			# 求直线和二次约束圆的交点
			inp = line_intersect_circle( l_h, [0,0,v_max])
			for inn in inp:
				h_point.append(inn)
			if v_flag:
				h_point.append(perf_point)
			for bh in B:
				# 求h_i与B中直线的交点
				l_point, l_flag = get_cross_point(l_h, bh[0:3])
				v_flag = point_in_B(l_point, l_flag, v_max, B)
				if v_flag:
					h_point.append(l_point)
			# 满足所有约束的交点保存下来 求出令目标函数最小的点
			if len(h_point) >= 1:
				v_min = v_max
				for hp in h_point:
					# print('hp', hp)
					hpp = np.array([hp[0], hp[1], hp[2]])
					vv = norm(hpp - v_perf)
					if vv < v_min:
						v_min = vv
						h_point_min = hpp
				v = h_point_min  # 更新v为p
			else:
				print('规划问题不可行，退出')
				break
		B.append(h)  # H_{i-1}
	# v是最终的解v_new
	return v


def point_in_B(l_point, l_flag, v_max, B):
	'''
	判断点是否在约束B内
	:param l_point:
	:param l_flag:
	:param v_max:
	:param B:
	:return:
	'''
	v_flag = False
	if l_flag:
		# 如果有交点 判断是否满足B中所有约束
		if norm(l_point) <= v_max:
			# 是否满足二次约束
			v_flag = True
		else:
			v_flag = False
		for vh in B:
			v_flag = in_half_plane(vh, l_point)
			if v_flag == False:
				break
		if v_flag:
			return v_flag
	return v_flag

# -------------------------------ORCA END---------------------------
# p = LineIntersectCircle([1, 0, -5], [0, 0, 10])
# print('points is {}'.format(p))

# point, flag = get_cross_point(np.array([1, 0, -5]), np.array([0, 1, 0]))
# print("point {} and flag {}".format(point, flag))
# 测试u的求解效果
lim = 300  # 限制坐标
lim_v = 3 # 限制速度

r_a = 20
r_b = 20
# 速度va
v_opt_a = np.array([1, 1, 0])
# 速度vb
v_opt_b_all = np.random.rand(50, 3)
v_opt_b_all = v_opt_b_all * lim_v * 2 - lim_v
v_opt_b_all[:, 2] = 0
# v_opt_b_all = np.array([[-15, -15, 0],
#                         [2, 0.5, 0],
#                         [-1, -6, 0],
#                         [1, 3, 0],
#                         [2, 1, 0],
#                         [-1, 2.5]])
# a的位置
pa = np.array([-1.2*lim, -1.2*lim, 0])
# b的位置
pb_all = np.random.rand(50, 3)
pb_all = pb_all * lim * 2 - 0.8 * lim
pb_all[:, 2] = 0
print('pb_all is :', pb_all)
# pb_all = np.array([[40, 40, 0],
#                    [51, 19, 0],
#                    [-70, 10, 0],
#                    [16, -40, 0],
#                    [88, -49, 0],
#                    [-20, -20, 0]])

v_max = 10
v_perf = np.zeros(3, dtype=float)
target = np.array([150, 150, 0])

tau = 10
count = 100

t_time = 0
h_time = 1
all_time = 1000
plt.ion()  # 开启绘图
while t_time < all_time:
	# plt.subplot(2, 1, 1)

	# plt.title('速度坐标系下的绘图')
	# plt.xlabel('vx')
	# plt.ylabel('vy')

	H = []  # 保存所有半平面

	for i in range(len(pb_all)):
		pb = pb_all[i]
		v_opt_b = v_opt_b_all[i]
		# print('v_opt_b', v_opt_b)
		v_opt = v_opt_a - v_opt_b
		# print('v_opt', v_opt)
		circle = np.array([(pb[0] - pa[0]) / tau, (pb[1] - pa[1]) / tau, (r_a + r_b) / tau])
		# print('circle', circle)
		u_vec, u_point, uflag, ll, lr = get_u_orca(v_opt=v_opt, circle=circle)
		print('u_vec, u_point, flag is :', u_vec, u_point, uflag)

		# plt.plot([0, v_opt[0]],[0, v_opt[1]])
		# plt.plot(circle[0], circle[1], 'o')

		# 画圆的点
		# x_c, y_c = get_circle([circle[0], circle[1]], circle[2])
		# plt.plot(x_c, y_c)

		# 画射线
		# print('ll, lr', ll, lr)
		if abs(ll[0]) > 1e-5:
			x_ll = np.linspace(-lim, lim, count)
			y_ll = ll[1] / ll[0] * x_ll
		else:
			x_ll = np.zeros(count)
			y_ll = np.linspace(-lim, lim, count)
		# plt.plot(x_ll, y_ll)

		if abs(lr[0]) > 1e-5:
			x_lr = np.linspace(-lim, lim, count)
			y_lr = lr[1] / lr[0] * x_lr
		else:
			x_lr = np.zeros(count)
			y_lr = np.linspace(-lim, lim, count)
		# plt.plot(x_lr, y_lr)

		# 如果u flag不是False
		if uflag:

			# plt.plot(u_point[0], u_point[1], '*')
			#  画ORCA半平面
			re = get_orca_area(u_vec, u_point, v_opt_a)
			l = re[0]
			print('l', l)
			orca = np.array([l[0], l[1], l[2], u_vec[0], u_vec[1], u_vec[2]])  # 半平面直线和方向矢量
			H.append(orca)  # 添加所有的半平面
			x = np.linspace(-lim, lim, count)
			if abs(l[1]) < 1e-5:
				# 直线方程B = 0
				x = np.zeros(count) - l[2]
				y = np.linspace(-lim, lim, count)
			else:
				y = -l[0] / l[1] * x + -l[2] / l[1]
			plt.plot(x, y, '--')
			print('绘制orca平面')
		print('i', i)

		# record_plt[0,:] = v_opt
		# record_plt[1,:] = circle
		# record_plt[2, :] = pb
		# record_plt[3, :] = ll
		# record_plt[4, :] = lr
		# record_plt[5, :] = l
	# 记录数据绘图用
	# mydict['velocity'].append(record_plt)

	v_perf = 0.5 * v_max * (target - pa) / norm(target - pa)

	v_new = nbody_area(H, v_max, v_opt, v_perf)
	v_opt_a = v_new
	# print('v_new', v_new)

	# 绘制速度坐标系下的期望方向和实际方向
	# plt.plot([0, 10*v_perf[0]], [0, 10*v_perf[1]])
	# plt.plot([0, 10*v_new[0]], [0, 10*v_new[1]])

	# 绘制大地坐标系下位置变化
	# plt.plot(target[0], target[1], 'o')
	# plt.plot(pa[0], pa[1], 'x')
	# plt.plot([pa[0], pa[0]+v_new[0]], [pa[1], pa[1] + v_new[1]])

	t_time += h_time
	# 船更新位置
	pa = pa + h_time * v_new
	# 障碍物更新位置
	pb_all = pb_all + v_opt_b_all * h_time

	plt.xlim(-lim, lim)
	plt.ylim(-lim, lim)

	# plt.subplot(2,1,2)
	plt.clf()  # 清除上个绘图
	plt.plot(target[0], target[1], 'o')
	plt.plot(pa[0],pa[1],'x')
	plt.plot([pa[0], pa[0]+v_new[0]*h_time], [pa[1], pa[1]+v_new[1]*h_time])
	x_a, y_a = get_circle([pa[0], pa[1]], r_a)
	plt.plot(x_a, y_a)
	for pb in pb_all:

		x_b, y_b = get_circle([pb[0], pb[1]], r_b)
		plt.plot(x_b, y_b)
	plt.xlim(-1.2*lim, 1.2*lim)
	plt.ylim(-1.2*lim, 1.2*lim)

	plt.pause(0.1)  # 暂停一秒
	if norm(target - pa) <= 10:
		print('到达目标了')
		break
	plt.ioff()  # 关闭画图的窗口

# plt.show()


# 结束循环
